#ifndef __FOCPIDCONFIG_H
#define __FOCPIDCONFIG_H

#include "foc.h"

extern volatile uint32_t gSpeedPID_TimeCNT;
extern volatile uint32_t gPositionPID_TimeCNT;

/******************************************************
 * 增量式PID结构体
 *****************************************************/
typedef struct
{
    float kp;        // 比例
    float kd;        // 微分
    float ki;        // 积分
    float tar;       // 目标
    float pre;       // 实际
    float bais;      // 目标与实际之前差值（目标-实际）
    float last_bias; // 上次差值
    float err;       // 累计差值
    float out;       // 输出
    float outMax;    // 输出限幅
    float cycle;     // 周期，单位秒
} SPID_Struct, *PSPID_Struct, PPID_Struct, *PPPID_Struct;

/******************************************************
 * 电机速度控制结构体
 *****************************************************/
typedef struct
{
    uint8_t isEnable;                     // 使能
    uint32_t sysTimer;                    // 系统时钟
    SPID_Struct sPID;                     // PID
    float (*GetPreSpeed)(void);           // 获取电机实际转速
    void (*SetOutCurrent)(float current); // 设置电机输出电流
} SpeedPIDControl_Struct, *PSpeedPIDControl_Struct;

#define SPEED_PID_CONTROL_EXPORT(x, xCycle, xOutMax, xGetPreSpeed, xSetOutCurrent) \
    SpeedPIDControl_Struct x = {                                                   \
        .isEnable = 0,                                                             \
        .sysTimer = 0,                                                             \
        .sPID.kp = 0,                                                              \
        .sPID.kd = 0,                                                              \
        .sPID.ki = 0,                                                              \
        .sPID.tar = 0,                                                             \
        .sPID.pre = 0,                                                             \
        .sPID.bais = 0,                                                            \
        .sPID.last_bias = 0,                                                       \
        .sPID.err = 0,                                                             \
        .sPID.out = 0,                                                             \
        .sPID.outMax = xOutMax,                                                    \
        .sPID.cycle = xCycle,                                                      \
        .GetPreSpeed = xGetPreSpeed,                                               \
        .SetOutCurrent = xSetOutCurrent,                                           \
    };

#define SPEEDPID_TIMEBASE(ms)   \
    do                          \
    {                           \
        gSpeedPID_TimeCNT += ms \
    } while (0)
#define SPEEDPID_TIMEOUT(timeOut, startTime) ((gSpeedPID_TimeCNT - startTime) >= timeOut ? 1 : 0)


#define SPEEDPID_GETTIME(void)                        \
    gSpeedPID_TimeCNT

/******************************************************
 * 电机位置控制结构体
 *****************************************************/
typedef struct
{
    uint8_t isEnable;                   // 使能
    uint32_t sysTimer;                  // 系统时钟
    PPID_Struct pPID;                   // PID
    float (*GetPrePosition)(void);      // 获取电机实际转速
    void (*SetMotorSpeed)(float speed); // 设置电机输出电流
}PositionPIDControl_Struct, *PPositionPIDControl_Struct;




/*************************************************************
** Function name:       POSITION_PID_CONTROL_EXPORT
** Descriptions:        声明一个位置控制对象
** Input parameters:    x:对象名字
**                      xCycle：PID控制周期 单位秒
**                      xGetMotorPrePosition：获取实际位置
**                      xSetMotorSpeed：设置电流输出
** Output parameters:   None
** Returned value:      None
** Remarks:             None
*************************************************************/
#define POSITION_PID_CONTROL_EXPORT(x,xCycle,xOutMax,xGetMotorPrePosition,xSetMotorSpeed)           \
PositionPIDControl_Struct x = {                                                                    \
    .isEnable = 0,                                                                              \
    .sysTimer = 0,                                                                              \
    .pPID.kp = 0,                                                                               \
    .pPID.kd = 0,                                                                               \
    .pPID.ki = 0,                                                                               \
    .pPID.tar = 0,                                                                              \
    .pPID.pre = 0,                                                                              \
    .pPID.bais = 0,                                                                             \
    .pPID.last_bias = 0,                                                                        \
    .pPID.err = 0,                                                                              \
    .pPID.out = 0,                                                                              \
    .pPID.outMax = xOutMax,                                                                     \
    .pPID.cycle = xCycle,                                                                       \
    .GetPrePosition = xGetMotorPrePosition,                                                     \
    .SetMotorSpeed = xSetMotorSpeed,                                                            \
};

/*************************************************************
** Function name:       MotorControl_TIMEBASE
** Descriptions:        时基,放在周期为1ms的函数里面执行
** Input parameters:    None
** Output parameters:   None
** Returned value:      None
*************************************************************/
#define POSITIONPID_TIMEBASE(ms)   \
    do                             \
    {                              \
        gPositionPID_TimeCNT += ms \
    } while (0)

#define POSITIONPID_GETTIME(void)  gPositionPID_TimeCNT


/*************************************************************
** Function name:       MotorControl_TIMEOUT
** Descriptions:        检查超时
** Input parameters:    timeOut：(uint32_t)超时时间
**                      startTime:(uint32_t)开始的时间
** Output parameters:   None
** Returned value:      false,未超时，true，超时
*************************************************************/
#define POSITIONPID_TIMEOUT(timeOut,startTime) ((gPositionPID_TimeCNT - startTime) >= timeOut ? 1 : 0)









void FOCPrintf(PFOC_Struct pFOC);
void SetCurrentPIDTar(PFOC_Struct pFOC, float tarid, float tariq);
void SetCurrentPIDTar(PFOC_Struct pFOC, float tarid, float tariq);
void CurrentPIControlID(PFOC_Struct pFOC);
void CurrentPIControlIQ(PFOC_Struct pFOC);

void SpeedPIDCycle(PSpeedPIDControl_Struct pSpeedPID);
void SetSpeedPIDParams(PSpeedPIDControl_Struct pSpeedPID, float kp, float ki, float kd);
void SetSpeedPIDTar(PSpeedPIDControl_Struct pSpeedPID, float tar);
void SetSpeedPIDEnable(PSpeedPIDControl_Struct pSpeedPID, float isEnable);
void SpeedPIDPrintf(PSpeedPIDControl_Struct pSpeedPID);

void PositionPIDCycle(PPositionPIDControl_Struct pPositionPID);
void SetPositionPIDParams(PPositionPIDControl_Struct pPositionPID,float kp,float ki,float kd);
void SetPositionPIDTar(PPositionPIDControl_Struct pPositionPID,float tar);
void SetPositionPIDEnable(PPositionPIDControl_Struct pPositionPID,float isEnable);
void PositionPIDPrintf(PPositionPIDControl_Struct pPositionPID);



#endif